% reference
% https://ww2.mathworks.cn/help/mcb/gs/pmsm-constraint-curves-and-their-application.html
% in the newer MATLAB versions with 'Motor Control Blockset'
% execute 'openExample('mcb/PMSMConstraintCurvesAndTheirApplicationExample')'

%% Example provided by MATLAB
% pmsm=mcb_SetPMSMMotorParameters('Teknic2310P');
% pmsm.Rs=0.01;
% pmsm.I_rated=40;
% pmsm.B=9e-5;
% pmsm.Lq=pmsm.Ld*2;
% inverter=mcb_SetInverterParameters('BoostXL-DRV8305');
% seed.drawLUTonConstraintCurves=1;
% seed.TBreakPointsCount=31;
% seed.wBreakPointsCount=32;
% PMSMLUT=mcbGenerateTables(pmsm,inverter,'idiqLUTs',seed);
% disp(PMSMLUT);

%% PMSM Parameters
PMSM_Param.Rs = 0.01; % Resistance of the stator windings
PMSM_Param.Ld = 2e-4; % d-axis inductances
PMSM_Param.Lq = 4e-4; % q-axis inductances
PMSM_Param.lambda = 0.0064; % Amplitude of the flux induced by the permanent magnets of the rotor in the stator phases
PMSM_Param.p = 4; % Numbers of Pole Pairs
PMSM_Param.F = 9e-5; % Combined viscous friction of rotor and load
PMSM_Param.Irated = 40;
Udc = 24;

%% Plot PMSM Constraint Curves
% approximate equations (resistance small enough):
% vd=-we*Lq*iq; vq=we*Ld*id+we*lambda; Umax=Udc/sqrt(3)
syms id iq omega_m time
eqn = id^2 + PMSM_Param.lambda / (PMSM_Param.Ld - PMSM_Param.Lq) * id - iq^2 == 0;
MTPA_id = solve(eqn, id);
eqn = PMSM_Param.Lq^2*iq^2 - (PMSM_Param.Ld*id+PMSM_Param.lambda)^2 ...
    - PMSM_Param.Lq*PMSM_Param.lambda/(PMSM_Param.Ld-PMSM_Param.Lq)*(PMSM_Param.Ld*id+PMSM_Param.lambda) == 0;
MTPV_id = solve(eqn, id);
figure
t = linspace(0, 2*pi, 100);
a = PMSM_Param.Irated;
x1 = a .* cos(t);
y1 = a .* sin(t);
plot(x1, y1); hold on % current limit
iq_trace = linspace(-40, 40, 401);
MTPA_id_trace = double(subs(MTPA_id, iq, iq_trace));
MTPA_id_trace = MTPA_id_trace(MTPA_id_trace(:, 1)<0, :);
plot(MTPA_id_trace, iq_trace); % MTPA trace
MTPV_id_trace = double(subs(MTPV_id, iq, iq_trace));
MTPV_id_trace = MTPV_id_trace(MTPV_id_trace(:, 1)<0, :);
plot(MTPV_id_trace, iq_trace); % MTPV trace
eqns_appro = [PMSM_Param.Lq^2*iq^2 - (PMSM_Param.Ld*id+PMSM_Param.lambda)^2 ...
    - PMSM_Param.Lq*PMSM_Param.lambda/(PMSM_Param.Ld-PMSM_Param.Lq)*(PMSM_Param.Ld*id+PMSM_Param.lambda) == 0, ...
    PMSM_Param.F*omega_m - 1.5*PMSM_Param.p*iq*(PMSM_Param.lambda+(PMSM_Param.Ld-PMSM_Param.Lq)*id) == 0, ...
    (PMSM_Param.Lq*iq)^2 + (PMSM_Param.Ld*id+PMSM_Param.lambda)^2 - Udc^2/3/PMSM_Param.p^2/omega_m^2 == 0];
Nmax_result = solve(eqns_appro, [id, iq, omega_m]);
index = (double(Nmax_result.id)<0) & (double(Nmax_result.iq)>0) & (double(Nmax_result.omega_m)>0);
omega_m_corner = double(Nmax_result.omega_m(index));
Te_zero = @(iq) (PMSM_Param.F*omega_m_corner/1.5/PMSM_Param.p./iq - PMSM_Param.lambda) ...
    ./ (PMSM_Param.Ld-PMSM_Param.Lq);
plot(Te_zero(iq_trace(iq_trace>0)), iq_trace(iq_trace>0)); % torque trace with zero load, only friction 
b = Udc / sqrt(3) / omega_m_corner / PMSM_Param.p;
x2 = (b .* cos(t) - PMSM_Param.lambda) ./ PMSM_Param.Ld;
y2 = b .* sin(t) ./ PMSM_Param.Lq;
plot(x2, y2); % approximate voltage limit
% axes settings
ax = gca; f = gcf;
ax.XLim = [-40, 0];
ax.YLim = [0, 40];
x_range = ax.Position(4) / (f.Position(3) / f.Position(4));
x_bias = (ax.Position(3) - x_range) / 2;
ax.Position = [ax.Position(1)+x_bias, ax.Position(2), ...
    x_range, ax.Position(4)];
xlabel('d-axis current (A)');
ylabel('q-axis current (A)');

% actual equations:
% vd=Rs*id-we*Lq*iq; vq=Rs*iq+we*Ld*id+we*lambda; Umax=Udc/sqrt(3)
figure
t = linspace(0, 2*pi, 100);
a = PMSM_Param.Irated;
x1 = a .* cos(t);
y1 = a .* sin(t);
plot(x1, y1); hold on % current limit
iq_trace = linspace(-40, 40, 401);
MTPA_id_trace = double(subs(MTPA_id, iq, iq_trace));
MTPA_id_trace = MTPA_id_trace(MTPA_id_trace(:, 1)<0, :);
plot(MTPA_id_trace, iq_trace); % MTPA trace
MTPV_id_trace = double(subs(MTPV_id, iq, iq_trace));
MTPV_id_trace = MTPV_id_trace(MTPV_id_trace(:, 1)<0, :);
plot(MTPV_id_trace, iq_trace); % MTPV trace
eqns_prime = [(PMSM_Param.Ld-PMSM_Param.Lq)*(PMSM_Param.Rs^2 + PMSM_Param.Ld^2*omega_m^2*PMSM_Param.p^2)*id^2 ...
    - (PMSM_Param.Ld-PMSM_Param.Lq)*(PMSM_Param.Rs^2 + PMSM_Param.Lq^2*omega_m^2*PMSM_Param.p^2)*iq^2 ...
    + PMSM_Param.Ld*(2*PMSM_Param.Ld-PMSM_Param.Lq)*PMSM_Param.lambda*omega_m^2*PMSM_Param.p^2*id ...
    + PMSM_Param.Rs^2*PMSM_Param.lambda*id + PMSM_Param.Ld*omega_m^2*PMSM_Param.p^2*PMSM_Param.lambda^2 == 0, ...
    PMSM_Param.F*omega_m - 1.5*PMSM_Param.p*iq*(PMSM_Param.lambda+(PMSM_Param.Ld-PMSM_Param.Lq)*id) == 0, ...
    (PMSM_Param.Rs*id - PMSM_Param.Lq*omega_m*PMSM_Param.p*iq)^2 ...
    + (PMSM_Param.Rs*iq + PMSM_Param.Ld*omega_m*PMSM_Param.p*id + PMSM_Param.lambda*omega_m*PMSM_Param.p)^2 ...
    - Udc^2/3 == 0];
Nmax_result = solve(eqns_prime, [id, iq, omega_m]);
index = (double(Nmax_result.id)<0) & (double(Nmax_result.iq)>0) & (double(Nmax_result.omega_m)>0);
omega_m_corner = double(Nmax_result.omega_m(index));
Te_zero = @(iq) (PMSM_Param.F*omega_m_corner/1.5/PMSM_Param.p./iq - PMSM_Param.lambda) ...
    ./ (PMSM_Param.Ld-PMSM_Param.Lq);
plot(Te_zero(iq_trace(iq_trace>0)), iq_trace(iq_trace>0)); % torque trace with zero load, only friction 
actual_VoltageEquation = [PMSM_Param.Rs*id - PMSM_Param.Lq*omega_m_corner*PMSM_Param.p*iq == Udc/sqrt(3)*cos(time) ...
    PMSM_Param.Rs*iq + PMSM_Param.Ld*omega_m_corner*PMSM_Param.p*id + PMSM_Param.lambda*omega_m_corner*PMSM_Param.p == Udc/sqrt(3)*sin(time)];
actual_VoltageTrace = solve(actual_VoltageEquation, [id, iq]);
x2 = double(subs(actual_VoltageTrace.id, time, t));
y2 = double(subs(actual_VoltageTrace.iq, time, t));
plot(x2, y2); % actual voltage limit
% axes settings
ax = gca; f = gcf;
ax.XLim = [-40, 0];
ax.YLim = [0, 40];
x_range = ax.Position(4) / (f.Position(3) / f.Position(4));
x_bias = (ax.Position(3) - x_range) / 2;
ax.Position = [ax.Position(1)+x_bias, ax.Position(2), ...
    x_range, ax.Position(4)];
xlabel('d-axis current (A)');
ylabel('q-axis current (A)');
